//
// File: EKF_weight_v3.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 20-Jun-2024 17:16:34
//

// Include Files
#include "EKF_weight_v3.h"
// #include <algorithm>
#include <cmath>
#include <cstring>

// Variable Definitions
static double P0[1024];

static const double dv[1024]{
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.01, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.01, 0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0, 0.0,  0.0, 0.0,
    0.0,  0.0, 0.0, 0.01};

// Function Definitions
//
// Arguments    : const double S1_data[]
//                const int S1_size[2]
//                const double Fnk[16]
//                const double Outputs[2]
//                const double a_pre[32]
//                double v_o
//                double w_o
//                double b_a[32]
// Return Type  : void
//
void EKF_weight_v3(const double S1_data[], const int S1_size[2],
                   const double Fnk[16], const double Outputs[2],
                   const double a_pre[32], double v_o, double w_o,
                   double b_a[32])
{
  double c_I[1024];
  double d_I[1024];
  double K[64];
  double O_k[64];
  double b_O_k[64];
  double b_y[64];
  double b_dv[32];
  double dw[32];
  double B[4];
  double b_v_o[2];
  double a21;
  double a22;
  double y;
  int Index;
  int dv_tmp;
  int r2;
  int vlen;
  signed char b_I[1024];
  //  初始化静态变量
  // 生成雅可比矩阵
  std::memset(&b_dv[0], 0, 32U * sizeof(double));
  std::memset(&dw[0], 0, 32U * sizeof(double));
  Index = 0;
  r2 = S1_size[1];
  for (int i{0}; i < 2; i++) {
    a21 = 0.3 * Outputs[i] + 1.0;
    for (int j{0}; j < 16; j++) {
      y = S1_data[0];
      for (int k{2}; k <= r2; k++) {
        y += S1_data[k - 1];
      }
      dv_tmp = Index + j;
      b_dv[dv_tmp] = Fnk[j] * y / (2.0 * (a21 * a21));
      if (i + 1 == 1) {
        a22 = 0.3 * Outputs[0] + 1.0;
        vlen = S1_size[1];
        y = S1_data[0];
        for (int k{2}; k <= vlen; k++) {
          y += S1_data[k - 1];
        }
        dw[dv_tmp] = Fnk[j] * y / (0.196 * (a22 * a22));
      } else {
        a22 = 0.3 * Outputs[1] + 1.0;
        vlen = S1_size[1];
        y = S1_data[0];
        for (int k{2}; k <= vlen; k++) {
          y += S1_data[k - 1];
        }
        dw[dv_tmp] = -(Fnk[j] * y) / (0.196 * (a22 * a22));
      }
    }
    Index += 16;
  }
  for (vlen = 0; vlen < 32; vlen++) {
    Index = vlen << 1;
    O_k[Index] = b_dv[vlen];
    O_k[Index + 1] = dw[vlen];
  }
  //  观测噪声协方差矩阵 R
  //  过程噪声协方差矩阵 Q
  //  卡尔曼增益 K
  for (vlen = 0; vlen < 2; vlen++) {
    for (r2 = 0; r2 < 32; r2++) {
      K[r2 + (vlen << 5)] = O_k[vlen + (r2 << 1)];
    }
  }
  for (vlen = 0; vlen < 32; vlen++) {
    for (r2 = 0; r2 < 2; r2++) {
      a21 = 0.0;
      for (dv_tmp = 0; dv_tmp < 32; dv_tmp++) {
        a21 += P0[vlen + (dv_tmp << 5)] * K[dv_tmp + (r2 << 5)];
      }
      b_y[vlen + (r2 << 5)] = a21;
    }
  }
  for (vlen = 0; vlen < 2; vlen++) {
    for (r2 = 0; r2 < 32; r2++) {
      a21 = 0.0;
      for (dv_tmp = 0; dv_tmp < 32; dv_tmp++) {
        a21 += O_k[vlen + (dv_tmp << 1)] * P0[dv_tmp + (r2 << 5)];
      }
      b_O_k[vlen + (r2 << 1)] = a21;
    }
    for (r2 = 0; r2 < 2; r2++) {
      a21 = 0.0;
      for (dv_tmp = 0; dv_tmp < 32; dv_tmp++) {
        a21 += b_O_k[vlen + (dv_tmp << 1)] * K[dv_tmp + (r2 << 5)];
      }
      B[vlen + (r2 << 1)] = a21 + 0.01;
    }
  }
  if (std::abs(B[1]) > std::abs(B[0])) {
    Index = 1;
    r2 = 0;
  } else {
    Index = 0;
    r2 = 1;
  }
  a21 = B[r2] / B[Index];
  y = B[Index + 2];
  a22 = B[r2 + 2] - a21 * y;
  for (int k{0}; k < 32; k++) {
    dv_tmp = k + (Index << 5);
    K[dv_tmp] = b_y[k] / B[Index];
    vlen = k + (r2 << 5);
    K[vlen] = (b_y[k + 32] - K[dv_tmp] * y) / a22;
    K[dv_tmp] -= K[vlen] * a21;
  }
  // step end
  //
  // 更新权重和P
  b_v_o[0] = v_o;
  b_v_o[1] = w_o;
  for (vlen = 0; vlen < 2; vlen++) {
    a21 = 0.0;
    for (r2 = 0; r2 < 32; r2++) {
      a21 += O_k[vlen + (r2 << 1)] * a_pre[r2];
    }
    b_v_o[vlen] -= a21;
  }
  a21 = b_v_o[0];
  y = b_v_o[1];
  for (vlen = 0; vlen < 32; vlen++) {
    b_a[vlen] = a_pre[vlen] + (K[vlen] * a21 + K[vlen + 32] * y);
  }
  std::memset(&b_I[0], 0, 1024U * sizeof(signed char));
  for (int k{0}; k < 32; k++) {
    b_I[k + (k << 5)] = 1;
  }
  for (vlen = 0; vlen < 32; vlen++) {
    a21 = K[vlen];
    y = K[vlen + 32];
    for (r2 = 0; r2 < 32; r2++) {
      dv_tmp = r2 << 1;
      Index = vlen + (r2 << 5);
      c_I[Index] = static_cast<double>(b_I[Index]) -
                   (a21 * O_k[dv_tmp] + y * O_k[dv_tmp + 1]);
    }
    for (r2 = 0; r2 < 32; r2++) {
      a21 = 0.0;
      for (dv_tmp = 0; dv_tmp < 32; dv_tmp++) {
        a21 += c_I[vlen + (dv_tmp << 5)] * P0[dv_tmp + (r2 << 5)];
      }
      Index = vlen + (r2 << 5);
      d_I[Index] = a21 + dv[Index];
    }
  }
  std::copy(&d_I[0], &d_I[1024], &P0[0]);
  //
  // step end
}

//
// Arguments    : void
// Return Type  : void
//
void EKF_weight_v3_init()
{
  std::copy(&dv[0], &dv[1024], &P0[0]);
}

//
// File trailer for EKF_weight_v3.cpp
//
// [EOF]
//
